/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
* 
*  \class	CScanMeasurementSet
*  \author 	Jari Saarinen
*  \date 	Mon Jun 12 22:16:05 2006
*  
*  This is a container for one laser scan measurement file 
*  The class reads a log file and stores Position and Ranging information
* 
*  Supported formats:
*  - LnMlib format (a file where on one line there is [x y z r1 a1 r2 a2 ... rN aN]
*  - MaCI logfiles
**/

#ifndef CSCANMEASUREMENTSET_H_
#define CSCANMEASUREMENTSET_H_

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <vector>
#include "TOccupancyGrid.h"
#include "RangingServer.hpp"
#include "PositionServer.hpp"
#include "PositionFileWriter.hpp"
#include "owndebug.h"
#include "ownutils.h"
#include "gimutils.h"
#include "RangingFileReader.hpp"
#include "PositionFileReader.hpp"

using namespace std;
class CScanMeasurementSet{
	public:
		
		CScanMeasurementSet(const char *logfilename); ///This is for LnMformat
		CScanMeasurementSet(const char *positionfile, const char *rangingfile); /// This is for macidata
		
		~CScanMeasurementSet();
		/**
		* Get Measurement from index ind
		* @param ind index of the measurement
		* @param &pos[out] position of measurement i is stored here
		* @param &s[out] scan of measurement i is stored here
		* @return true, if success
		*/
		bool getMeasurement(int ind, Grid::pose &pos, Grid::TScan &s);
		/**
		* Get Measurement from index ind and the transformed (in 2D) to laser frame
		* @param ind index of the measurement
		* @param &pos[out] position of measurement i is stored here
		* @param &s[out] scan of measurement i is stored here
		* @return true, if success
		*/
		bool getMeasurementInLaserFrame(int ind, Grid::pose &pos, Grid::TScan &s);
		bool setMeasurementToRobotFrame(int ind, Grid::pose lpos);
		
		bool getPoseInLaserFrame(int ind, Grid::pose &pos);
		
		/**
		* Converts a pose from robot frame to laser frame
		*/
		bool convertToLaserFrame(Grid::pose &pos);
		
		int size(){return poses.size();}
		
		bool saveLnMFormat(const char *fname);
		
		/**
		* Saves the Position vector in MaCI logfile format
		*/
		bool saveMaCIPositionFILE(std::string aFileName,
															std::string aMaCIGroup,
															std::string aInstanceName);
		
		std::vector< std::vector<float> > ranges; ///All scans
		std::vector< float > angles; ///Angles (same for all)
		std::vector< Grid::pose > poses;    ///All poses for previous map
		int numMeas;
		float lx,ly,lz,la; ///< laser position with respect to position
		
		///Laser measurement Timestaps 
		std::vector<gim::time> rTimes;
		
		MaCI::Ranging::TDeviceInformationLimits deviceLimits;

		
	private:
		int forwardString(const char *buf, int begin,int items);
		int readScanFile(const char *fname);
		bool readMaciDataFiles(const char *positionFile, const char *rangingFile);
		
};

#endif

